#include "TrackWidget.h"

TrackWidget::TrackWidget(QWidget *parent) : QWidget(parent)
{
    isInitState = true;
    nodeList = new QList<BaseNode *>;
    interpolationPoints.clear();
    updateTimer = new QTimer;
    connect(updateTimer, SIGNAL(timeout()), this, SLOT(update()));
    updateTimer->start(50);
}

void TrackWidget::paintEvent(QPaintEvent *event)
{
    Q_UNUSED(event)
    if (isInitState)
    {
        drawTipText("点击仿真按钮显示图像");
    }
    else
    {
        drawTipText("正在加载……");
        drawNodePath();
    }
}

void TrackWidget::resetDrawing() // 回复原始状态
{
    isInitState = true;
    update();
}

void TrackWidget::drawTipText(QString tip) // 显示文字
{
    QPainter painter(this);
    // background
    painter.setPen(Qt::NoPen);
    painter.setBrush(QColor(191, 239, 255));
    painter.drawRect(rect());
    // text
    QPen txtPen(QColor(Qt::darkBlue));
    painter.setPen(txtPen);
    painter.drawText(width()/2 - 200/2, height()/2 - 30/2,
                     200, 30, Qt::AlignCenter, tip);
}

void TrackWidget::drawPath(const QList<BaseNode *>& nodeList, // 外部调用
                           const PathGeometry& geo)
{
    *(this->nodeList) = nodeList;
    this->geo = geo;
    isInitState = false;
    interpolationPoints.clear();
    update();
}

void TrackWidget::drawNodePath() // 绘制路径
{
    QPainter painter(this);
    // background
    painter.setPen(Qt::NoPen);
    painter.setBrush(QColor(191, 239, 255));
    painter.drawRect(rect());
    // path
    // 需要缩小投影
    QPen pathPen(QColor(Qt::darkBlue));
    pathPen.setWidth(3);
    painter.setPen(pathPen);
    painter.setRenderHint(QPainter::HighQualityAntialiasing);
    if (nodeList->size() > 1000)
    {
        drawTipText("路径太大 无法预览");
        return;
    }
    double ratio = findProjectionRatio(); // 计算缩放比例
    double current_x, current_y; // 绘图坐标系下的当前点跟踪
    findStartPoint(current_x, current_y, ratio); // 计算绘制起点
    QPointF arrowEnd; // 保存绘制箭头的位置
    for (int i = 0; i < nodeList->size(); ++i) // 遍历绘制每一个node
    {
        if (i == 0) // need arrow
        {
            arrowEnd.rx() = current_x;
            arrowEnd.ry() = current_y;
        }
        drawOneNode(&painter, nodeList->at(i), ratio, current_x, current_y);
    }
    // 绘制插补路径
    drawInterpPath(&painter, ratio, arrowEnd);
    // 绘制表示加工方向的箭头
    int firstNode = 0;
    drawStartDirection(&painter, nodeList->at(firstNode), ratio,
                       arrowEnd.x(), arrowEnd.y());
}

double TrackWidget::findProjectionRatio() // 计算缩放比例
{
    double width = geo.maxX - geo.minX; // 路径的尺寸
    double height = geo.maxY - geo.minY;
    double ratioWidth = width / (this->width() - 20); // -20是为了边缘留空白
    double ratioHeight = height / (this->height() - 20);
    return std::max(ratioHeight, ratioWidth);
}

void TrackWidget::findStartPoint(double &startX, double &startY, double ratio) // 计算起点
{
    double realHeigth = (geo.maxY - geo.minY) / ratio;
    double realWidth = (geo.maxX - geo.minX) / ratio;
    int originY = (height() - realHeigth) / 2; // 绘图坐标系原点
    int originX = (width() - realWidth) / 2;
    startX = -geo.minX / ratio + originX;
    startY = geo.maxY / ratio + originY;
}

void TrackWidget::drawOneNode(QPainter *p, const BaseNode *node, // 绘制一段
                              double ratio,
                              double &current_x, double &current_y)
{
    if (node->getKind() == PK_LINE)
    {
        double end_x = ((LineNode*)node)->endPoint.x() / ratio + current_x;
        double end_y = -((LineNode*)node)->endPoint.y() / ratio + current_y;
        p->drawLine(QPointF(current_x, current_y),QPointF(end_x, end_y));
        // update current position
        current_x = end_x;
        current_y = end_y;
    }
    else if (node->getKind() == PK_ARC)
    {
//        qDebug("ARC: center: %f, %f; angle: %f",
//               ((ArcNode*)node)->center.x, ((ArcNode*)node)->center.y,
//               ((ArcNode*)node)->angle);
        // angle unit of QPainter::drawArc is 1/16 degree
        int startAngle = std::round(std::atan2(-((ArcNode*)node)->_center.y(),
                                       -((ArcNode*)node)->_center.x()) / M_PI * 180 * 16);
        int swapAngle = std::round(((ArcNode*)node)->swapAngle / M_PI * 180 * 16);
        int radius = std::hypot(((ArcNode*)node)->_center.y(),
                                   ((ArcNode*)node)->_center.x()) / ratio;
        int upperLeftX = ((ArcNode*)node)->_center.x() / ratio
                                    + current_x - radius;
        int upperLeftY = -((ArcNode*)node)->_center.y() / ratio
                                    + current_y - radius;
//        qDebug("draw arc: startAngle: %d, swapAngle: %d, radius: %d, corner: %d, %d",
//               startAngle, swapAngle, radius, upperLeftX, upperLeftY);
        p->drawArc(QRectF(upperLeftX, upperLeftY, radius * 2, radius * 2),
                   startAngle, swapAngle);
        // update current position
        current_x = ((ArcNode*)node)->endPoint.x() / ratio + current_x;
        current_y = -((ArcNode*)node)->endPoint.y() / ratio + current_y;
    }
}

// draw an arrow to indicate the direction of the path
void TrackWidget::drawStartDirection(QPainter *p, const BaseNode *startNode,
                                        double ratio,
                                        const double &cur_x, const double &cur_y)
{
    ///@startNode is generally the second node. The first node is the cut-in node.

    // pen
    p->save(); // 保存之前的状态，以便画完箭头后恢复
    QPen arrowPen(QColor(Qt::yellow));
    arrowPen.setWidth(2);
    p->setPen(arrowPen);
    p->setRenderHint(QPainter::HighQualityAntialiasing);
    // size
    const int arrowLen = 20;
    const int arrowSide = 10;
    const double sideAngle = M_PI * 5 / 6;
    if (startNode->getKind() == PK_LINE)
    {
        int end_x = std::round(((LineNode*)startNode)->endPoint.x() / ratio + cur_x);
        int end_y = std::round(-((LineNode*)startNode)->endPoint.y() / ratio + cur_y);
        double lineAngle = std::atan2(end_y - cur_y, end_x - cur_x);
        double arrowPointX = arrowLen * std::cos(lineAngle) + cur_x;
        double arrowPointY = arrowLen * std::sin(lineAngle) + cur_y;
        double arrowSide1X = arrowSide * std::cos(lineAngle - sideAngle) + arrowPointX;
        double arrowSide1Y = arrowSide * std::sin(lineAngle - sideAngle) + arrowPointY;
        double arrowSide2X = arrowSide * std::cos(lineAngle + sideAngle) + arrowPointX;
        double arrowSide2Y = arrowSide * std::sin(lineAngle + sideAngle) + arrowPointY;
        p->drawLine(cur_x, cur_y, arrowPointX, arrowPointY);
        p->drawLine(arrowPointX, arrowPointY, arrowSide1X, arrowSide1Y);
        p->drawLine(arrowPointX, arrowPointY, arrowSide2X, arrowSide2Y);
    }
    else if (startNode->getKind() == PK_ARC)
    {
        // angle unit of QPainter::drawArc is 1/16 degree
        double startAngle = std::atan2(-((ArcNode*)startNode)->_center.y(),
                                       -((ArcNode*)startNode)->_center.x());
        startAngle = ((ArcNode*)startNode)->isCCW() ? startAngle + M_PI / 2 :
                                                      startAngle - M_PI / 2;
        startAngle *= -1; // because in this coordinate +y points down
        double arrowPointX = arrowLen * std::cos(startAngle) + cur_x;
        double arrowPointY = arrowLen * std::sin(startAngle) + cur_y;
        double arrowSide1X = arrowSide * std::cos(startAngle - sideAngle) + arrowPointX;
        double arrowSide1Y = arrowSide * std::sin(startAngle - sideAngle) + arrowPointY;
        double arrowSide2X = arrowSide * std::cos(startAngle + sideAngle) + arrowPointX;
        double arrowSide2Y = arrowSide * std::sin(startAngle + sideAngle) + arrowPointY;
        p->drawLine(cur_x, cur_y, arrowPointX, arrowPointY);
        p->drawLine(arrowPointX, arrowPointY, arrowSide1X, arrowSide1Y);
        p->drawLine(arrowPointX, arrowPointY, arrowSide2X, arrowSide2Y);
    }
    p->restore();
}

void TrackWidget::addInterpPoint(QPointF *point)
{
    if (interpolationPoints.isEmpty())
    {
        interpolationPoints.append(point);
        return;
    }
    if (fabs(interpolationPoints.last()->x() - point->x()) > 1e-5 ||
            fabs(interpolationPoints.last()->y() - point->y()) > 1e-5)
        interpolationPoints.append(point);
}

void TrackWidget::drawInterpPath(QPainter *p, double ratio, QPointF startPoint)
{
    p->save();
    QPen interpPen;
    interpPen.setWidth(2);
    interpPen.setColor(Qt::red);
    p->setPen(interpPen);
    QPointF start = startPoint;
//    qDebug() << "start" << start;
    QPointF end;
    if (interpolationPoints.size() > 1)
    for (int i = 1; i < interpolationPoints.size(); ++i)
    {
        QPointF delta = *(interpolationPoints.at(i)) - *(interpolationPoints.at(i-1));
        end.rx() = delta.x() / ratio + start.x();
        end.ry() = -delta.y() / ratio + start.y();
        p->drawLine(start, end);
        start = end;
    }
//    qDebug() << "line:" << startPoint << "->" <<end;
    interpPen.setWidth(3);
    interpPen.setColor(Qt::magenta);
    p->setPen(interpPen);
    p->drawArc(end.x() - 3, end.y() - 3, 6, 6, 0, 360 * 16);
    p->restore();
}
